#include <cstdio>
#include <cstring>
#include "livox.h"

//#include <thread>
#include <memory>
/** Const varible ------------------------------------------------------------------------------- */
/** User add broadcast code here */
boost::mutex LdsLidar::imu_mu;
boost::mutex LdsLidar::Clouds_mu;
// boost::mutex LdsLidar::FrameLock;
std::shared_ptr<Eigen::Vector3d> LdsLidar::macc;
std::shared_ptr<Eigen::Vector3d> LdsLidar::mgyro;
// std::vector<LivoxEthPacket *> RawClientDatas;
int LdsLidar::CloudsNum_;
std::deque<CloudI> LdsLidar::Iclouds;
std::deque<Gydata> LdsLidar::Gydatas;
Gydata LdsLidar::OriginRoT;
PcloudI LdsLidar::Icloud;
// cv::Mat LdsLidar::DepthImg;
static const char* local_broadcast_code_list[] = {"0TFDFG700601861", "3WEDH7600110891"};
/** For callback use only */
LdsLidar* g_lidars = nullptr;
Gyodometry& myodo = Gyodometry::GetInstance();
/** Lds lidar function ---------------------------------------------------------------------------*/
// LdsLidar::LdsLidar() : RawDatas(48 * 500)
LdsLidar::LdsLidar(bool isOdometry)
{
    if (isOdometry)
    {
        //创建数据收集线程
        std::thread myodoThread(&Gyodometry::DataCL, &myodo);
        myodoThread.detach();
        // myodo.DataCL();
        isOdometry_ = true;
    }

    //对旋转矩阵初赋值
    OriginRoT.Tran = Eigen::Vector3f(0., 0., 0.);
    OriginRoT.yaw = 0.;
    OriginRoT.sectime = 0;

    auto_connect_mode_ = true;
    whitelist_count_ = 0;
    is_initialized_ = false;
    lidar_count_ = 0;
    memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
    memset(lidars_, 0, sizeof(lidars_));
    const std::string cfgFile_path = "../YmlFiles/cfg.yml";
    YAML::Node MatrixFile = YAML::LoadFile(cfgFile_path);
    const std::vector<float> RoTVec = MatrixFile["RoT"].as<std::vector<float>>();
    const std::vector<float> CamVec = MatrixFile["CamIntrix"].as<std::vector<float>>();
    Eigen::Matrix<float, 4, 4, Eigen::RowMajor> TRoT(RoTVec.data());
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> TCam(CamVec.data());
    RoT.swap(TRoT);
    CamIntrix.swap(TCam);
    // RoT << -0.0237806, -0.999379, 0.0259964, 0.183787,
    //     -0.0103828, -0.0257555, -0.999614, 0.020738,
    //     0.999663, -0.0240413, -0.00976387, -0.369433,
    //     0, 0, 0, 1;
    // CamIntrix << 1263.963, 0.0, 620.6840,
    //     0.0, 1263.660, 532.8492,
    //     0, 0, 1.0;
    // CamIntrix << 1713.91138912005, 0.143349090450384, 627.578597951611,
    //     0, 1714.20429945400, 486.114880603001,
    //     0, 0, 1;

    // DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);
    for (uint32_t i = 0; i < kMaxLidarCount; i++)
    {
        lidars_[i].handle = kMaxLidarCount;
        /** Unallocated state */
        lidars_[i].connect_state = kConnectStateOff;
    }
}

LdsLidar::~LdsLidar()
{
}

int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs, int max_size, const int CloudsNum, bool Dynamic)
{
    if (is_initialized_)
    {
        printf("LiDAR data source is already inited!\n");
        return -1;
    }
    if (!Init())
    {
        Uninit();
        printf("Livox-SDK init fail!\n");
        return -1;
    }
    Dynamic_ = Dynamic;
    CloudsNum_ = CloudsNum;
    max_cloud_size = max_size;
    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->points.resize(max_cloud_size, pcl::PointXYZ(0, 0, 0));
    Icloud.reset(new CloudI());
    Icloud->points.resize(max_cloud_size, pcl::PointXYZI(0));

    LivoxSdkVersion _sdkversion;
    GetLivoxSdkVersion(&_sdkversion);
    printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
    SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
    SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);
    /** Add commandline input broadcast code */
    for (const auto& input_str : broadcast_code_strs)
    {
        LdsLidar::AddBroadcastCodeToWhitelist(input_str.c_str());
    }
    /** Add local broadcast code */
    LdsLidar::AddLocalBroadcastCode();
    if (whitelist_count_)
    {
        LdsLidar::DisableAutoConnectMode();
        printf("Disable auto connect mode!\n");

        printf("List all broadcast code in whiltelist:\n");
        for (uint32_t i = 0; i < whitelist_count_; i++)
        {
            printf("%s\n", broadcast_code_whitelist_[i]);
        }
    }
    else
    {
        LdsLidar::EnableAutoConnectMode();
        printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
    }
    /** Start livox sdk to receive lidar data */
    if (!Start())
    {
        Uninit();
        printf("Livox-SDK init fail!\n");
        return -1;
    }
    /** Add here, only for callback use */
    if (g_lidars == nullptr)
    {
        g_lidars = this;
    }
    is_initialized_ = true;
    printf("Livox-SDK init success!\n");
    return 0;
}

int LdsLidar::DeInitLdsLidar(void)
{
    if (!is_initialized_)
    {
        printf("LiDAR data source is not exit");
        return -1;
    }
    Uninit();
    printf("Livox SDK Deinit completely!\n");
    return 0;
}

void LdsLidar::LivoxImudata(LivoxImuPoint* p_point_data, uint32_t data_num, std::shared_ptr<Eigen::Vector3d>& gyro, std::shared_ptr<Eigen::Vector3d>& acc)
{
    gyro.reset(new Eigen::Vector3d(p_point_data->gyro_x, p_point_data->gyro_y, p_point_data->gyro_z));
    acc.reset(new Eigen::Vector3d(p_point_data->acc_x, p_point_data->acc_y, p_point_data->acc_z));
}

/*
 * woshishabi
 */
/** Static function in LdsLidar for callback or event process ------------------------------------*/
/** Receiving point cloud data from Livox LiDAR. */
void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data)
{
    using namespace std;
    // static int num2 = 0;
    static double starttime, endtime;
    static int num = 0;
    static int num_cloud = 0;
    static bool start = false;
    timeval time;
    if (!start)
    {
        gettimeofday(&time, NULL);
        starttime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
        start = true;
    }
    // if (num2 == 0)
    // {
    //     gettimeofday(&time, nullptr);
    //     starttime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
    // }
    //拷贝内存
    // auto* data_cp = (LivoxEthPacket*)malloc(sizeof(LivoxEthPacket) * data_num);
    // LivoxEthPacket* data_cpy = (LivoxEthPacket*)memcpy(data_cp, data, sizeof(LivoxEthPacket) * data_num);
    // static int num2 = 0;
    // LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
    LdsLidar* this_ = static_cast<LdsLidar*>(client_data);
    LivoxEthPacket* eth_packet = data;

    if (!data || !data_num || (handle >= kMaxLidarCount))
    {
        return;
    }

    // printf("%d\n",data_num);
    // else
    // {
    if (eth_packet)
    {
        this_->data_recveive_count_[handle]++;
        // this_->cloud_mu.lock();
        this_->cloud_mu.lock();
        /** Parsing the timestamp and the point cloud data. */
        // uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
        // printf("%lu\n",cur_timestamp);

        if (data->data_type == kCartesian)
        {
            // DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);
            for (size_t i = 0; i < data_num; i++)
            {
                // std::cout << "data_num is: " << data_num << std::endl;
                bool isEmpty = false;
                pcl::PointXYZ point;
                Point tPoint;
                PointI pointI;
                LivoxRawPoint* p_point_data = (LivoxRawPoint*)data->data + i;
                point.x = p_point_data->x / 1000.;
                point.y = p_point_data->y / 1000.;
                point.z = p_point_data->z / 1000.;
                this_->cloud->points[num] = point;
                pointI.x = p_point_data->x / 1000.;
                pointI.y = p_point_data->y / 1000.;
                pointI.z = p_point_data->z / 1000.;
                pointI.intensity = p_point_data->reflectivity;
                this_->Icloud->points[num] = pointI;
                num++;
                num = num % this_->max_cloud_size;
                // if(num == 0)
                //     DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);

                // if (point.x == 0 || point.x > 10)
                //     continue;
                // if (abs(point.y) > 6 || abs(point.z) > 1.7)
                //     continue;
                // Eigen::Vector4f PxCoor;
                // Eigen::Vector4f CamCoor;
                // CamCoor = RoT * Eigen::Vector4f(point.x, point.y, point.z, 1);
                //     PxCoor = CamIntrix * CamCoor;
                // float u, v;
                // u = PxCoor(0) / CamCoor(2);
                // v = PxCoor(1) / CamCoor(2);
                // if (u < 0 || u > 1280 || v < 0 || v > 1024)
                //     continue;

                // DepthImg.at<short>(v,u) = CamCoor(2) * 1000;
            }
        }

        else if (data->data_type == kSpherical)
        {
            LivoxSpherPoint* p_point_data = (LivoxSpherPoint*)data->data;
        }
        else if (data->data_type == kExtendCartesian)
        {
            LivoxExtendRawPoint* p_point_data = (LivoxExtendRawPoint*)data->data;
        }
        else if (data->data_type == kExtendSpherical)
        {
            LivoxExtendSpherPoint* p_point_data = (LivoxExtendSpherPoint*)data->data;
        }
        else if (data->data_type == kDualExtendCartesian)
        {
            // #if _OPENMP
            if (this_->Dynamic_ == true) // 动态建图
            {
#pragma omp parallel for num_threads(1)
                // #endif
                for (size_t i = 0; i < data_num; i++)
                {
                    pcl::PointXYZ point;
                    LivoxDualExtendRawPoint* p_point_data = (LivoxDualExtendRawPoint*)data->data + i;
                    PointI pointI;
                    //第一回波
                    // point.x = p_point_data->x1 / 1000.;
                    // point.y = p_point_data->y1 / 1000.;
                    // point.z = p_point_data->z1 / 1000.;
                    // this_->cloud->points[num] = point;
                    pointI.x = p_point_data->x1 / 1000.;
                    pointI.y = p_point_data->y1 / 1000.;
                    pointI.z = p_point_data->z1 / 1000.;
                    pointI.intensity = p_point_data->reflectivity1;
                    this_->Icloud->points[num] = pointI;
                    num++;
                    std::cout << pointI.x << "  " << std::endl;
                    //第二回波
                    // point.x = p_point_data->x2 / 1000.;
                    // point.y = p_point_data->y2 / 1000.;
                    // point.z = p_point_data->z2 / 1000.;
                    // this_->cloud->points[num] = point;

                    pointI.x = p_point_data->x2 / 1000.;
                    pointI.y = p_point_data->y2 / 1000.;
                    pointI.z = p_point_data->z2 / 1000.;
                    pointI.intensity = p_point_data->reflectivity2;
                    this_->Icloud->points[num] = pointI;
                    num++;
                    num = num % this_->max_cloud_size;
                    if (num == 0)
                    {
                        if (this_->isOdometry_)
                        {
                            // 获取陀螺仪数据
                            myodo.DataCB(OriginRoT);
                        }
                        gettimeofday(&time, NULL);
                        endtime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
                        // std::cout << "the time gap is: " << endtime - starttime << std::endl;
                        starttime = endtime;
                        if (num_cloud == this_->CloudsNum_)
                        {
                            // Clouds_mu.lock();
                            Gydatas.pop_front();
                            Gydatas.push_back(OriginRoT);
                            Iclouds.pop_front();
                            // CloudI temp;
                            // PcloudI temp(new CloudI());
                            // pcl::copyPointCloud(*(this_->Icloud), *temp);
                            Iclouds.push_back(*(this_->Icloud));
                            // Clouds_mu.unlock();
                            num_cloud--;
                        }
                        else
                        {
                            // Clouds_mu.lock();
                            Gydatas.push_back(OriginRoT);
                            // PcloudI temp(new CloudI());
                            // pcl::copyPointCloud(*(this_->Icloud), *temp);
                            Iclouds.push_back(*(this_->Icloud));
                            // Clouds_mu.unlock();
                        }
                        num_cloud++;
                    }
                }
            }

            else //静态建图
            {
                for (size_t i = 0; i < data_num; i++)
                {
                    pcl::PointXYZ point;
                    LivoxDualExtendRawPoint* p_point_data = (LivoxDualExtendRawPoint*)data->data + i;
                    PointI pointI;
                    //第一回波
                    point.x = p_point_data->x1 / 1000.;
                    point.y = p_point_data->y1 / 1000.;
                    point.z = p_point_data->z1 / 1000.;
                    this_->cloud->points[num] = point;

                    pointI.x = p_point_data->x1 / 1000.;
                    pointI.y = p_point_data->y1 / 1000.;
                    pointI.z = p_point_data->z1 / 1000.;
                    pointI.intensity = p_point_data->reflectivity1;
                    this_->Icloud->points[num] = pointI;
                    num++;
                    //第二回波
                    point.x = p_point_data->x2 / 1000.;
                    point.y = p_point_data->y2 / 1000.;
                    point.z = p_point_data->z2 / 1000.;
                    this_->cloud->points[num] = point;

                    pointI.x = p_point_data->x2 / 1000.;
                    pointI.y = p_point_data->y2 / 1000.;
                    pointI.z = p_point_data->z2 / 1000.;
                    pointI.intensity = p_point_data->reflectivity2;
                    this_->Icloud->points[num] = pointI;
                    num++;
                    num = num % this_->max_cloud_size;
                }
            }
        }
        else if (data->data_type == kDualExtendSpherical)
        {
            LivoxDualExtendSpherPoint* p_point_data = (LivoxDualExtendSpherPoint*)data->data;
        }
        else if (data->data_type == kImu)
        {
            LivoxImuPoint* p_point_data = (LivoxImuPoint*)data->data;
        }
        else if (data->data_type == kTripleExtendCartesian)
        {
            LivoxTripleExtendRawPoint* p_point_data = (LivoxTripleExtendRawPoint*)data->data;
        }
        else if (data->data_type == kTripleExtendSpherical)
        {
            LivoxTripleExtendSpherPoint* p_point_data = (LivoxTripleExtendSpherPoint*)data->data;
        } // else
        //        {
        //            printf("---\n");
        //        }
        else if (data->data_type = kImu)
        {
            LivoxImuPoint* p_point_data = (LivoxImuPoint*)data->data;
            std::shared_ptr<Eigen::Vector3d> gyro_tp, acc_tp;
            LivoxImudata(p_point_data, data_num, gyro_tp, acc_tp);
            if (gyro_tp && acc_tp)
            {
                imu_mu.lock();
                mgyro = gyro_tp;
                macc = acc_tp;
                imu_mu.unlock();
            }
        }
        this_->cloud_mu.unlock();
        // this_->cloud_mu.unlock();
        //   printf("there are %d packets of zero data of %d data\n",zero_num,data_num);
        // }
        // free(data_cp);
    }
}

void LdsLidar::getImuData(std::shared_ptr<Eigen::Vector3d>& gyro, std::shared_ptr<Eigen::Vector3d>& acc, bool destroy)
{
    if (destroy)
    {
        imu_mu.lock();
        auto temp1 = mgyro;
        auto temp2 = macc;
        mgyro.reset();
        macc.reset();
        gyro = temp1;
        acc = temp2;
    }
    else
    {
        imu_mu.lock();
        gyro = mgyro;
        acc = macc;
    }
    imu_mu.unlock();
}

void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo* info)
{
    if (info == nullptr)
    {
        return;
    }
    if (info->dev_type == kDeviceTypeHub)
    {
        printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
        return;
    }
    if (g_lidars->IsAutoConnectMode())
    {
        printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
    }
    else
    {
        if (!g_lidars->FindInWhitelist(info->broadcast_code))
        {
            printf("Not in the whitelist, please add %s to if want to connect!\n", info->broadcast_code);
            return;
        }
    }
    livox_status result = kStatusFailure;
    uint8_t handle = 0;
    result = AddLidarToConnect(info->broadcast_code, &handle);
    if (result == kStatusSuccess && handle < kMaxLidarCount)
    {
        LidarDevice* p_lidar = &(g_lidars->lidars_[handle]);
        p_lidar->handle = handle;
        p_lidar->connect_state = kConnectStateOff;
        p_lidar->config.enable_fan = true;
        p_lidar->config.return_mode = kDualReturn;
        // p_lidar->config.return_mode = kStrongestReturn;
        p_lidar->config.coordinate = kCoordinateCartesian;
        p_lidar->config.imu_rate = kImuFreq200Hz;
        SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void*)g_lidars);
    }
    else
    {
        printf("Add lidar to connect is failed : %d %d \n", result, handle);
    }
}

/** Callback function of changing of device state. */
void LdsLidar::OnDeviceChange(const DeviceInfo* info, DeviceEvent type)
{
    if (info == nullptr)
    {
        return;
    }
    uint8_t handle = info->handle;
    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice* p_lidar = &(g_lidars->lidars_[handle]);
    if (type == kEventConnect)
    {
        QueryDeviceInformation(handle, DeviceInformationCb, g_lidars);
        if (p_lidar->connect_state == kConnectStateOff)
        {
            p_lidar->connect_state = kConnectStateOn;
            p_lidar->info = *info;
        }
        printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
    }
    else if (type == kEventDisconnect)
    {
        p_lidar->connect_state = kConnectStateOff;
        printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
    }
    else if (type == kEventStateChange)
    {
        p_lidar->info = *info;
        printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
    }
    if (p_lidar->connect_state == kConnectStateOn)
    {
        printf("Device Working State %d\n", p_lidar->info.state);
        if (p_lidar->info.state == kLidarStateInit)
        {
            printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
        }
        else
        {
            printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
        }
        printf("Device feature %d\n", p_lidar->info.feature);
        SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
        /** Config lidar parameter */
        if (p_lidar->info.state == kLidarStateNormal)
        {
            if (p_lidar->config.coordinate != 0)
            {
                SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
            }
            else
            {
                SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
            }
            p_lidar->config.set_bits |= kConfigCoordinate;
            if (kDeviceTypeLidarMid40 != info->type)
            {
                LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode), LdsLidar::SetPointCloudReturnModeCb, g_lidars);
                p_lidar->config.set_bits |= kConfigReturnMode;
            }
            if (kDeviceTypeLidarMid40 != info->type && kDeviceTypeLidarMid70 != info->type)
            {
                LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate), LdsLidar::SetImuRatePushFrequencyCb, g_lidars);
                p_lidar->config.set_bits |= kConfigImuRate;
            }
            p_lidar->connect_state = kConnectStateConfig;
        }
    }
}

/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, DeviceInformationResponse* ack, void* client_data)
{
    if (status != kStatusSuccess)
    {
        printf("Device Query Informations Failed : %d\n", status);
    }
    if (ack)
    {
        printf("firm ver: %d.%d.%d.%d\n", ack->firmware_version[0], ack->firmware_version[1], ack->firmware_version[2], ack->firmware_version[3]);
    }
}

/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage* message)
{
    static uint32_t error_message_count = 0;
    if (message != nullptr)
    {
        ++error_message_count;
        if (0 == (error_message_count % 100))
        {
            printf("handle: %u\n", handle);
            printf("temp_status : %u\n", message->lidar_error_code.temp_status);
            printf("volt_status : %u\n", message->lidar_error_code.volt_status);
            printf("motor_status : %u\n", message->lidar_error_code.motor_status);
            printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
            printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
            printf("pps_status : %u\n", message->lidar_error_code.device_status);
            printf("fan_status : %u\n", message->lidar_error_code.fan_status);
            printf("self_heating : %u\n", message->lidar_error_code.self_heating);
            printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
            printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
            printf("system_status : %u\n", message->lidar_error_code.system_status);
        }
    }
}

void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
}

void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
    LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
        printf("Set return mode success!\n");
        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode), LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
        printf("Set return mode fail, try again!\n");
    }
}

void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
    LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
        printf("Set coordinate success!\n");

        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        if (p_lidar->config.coordinate != 0)
        {
            SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
        }
        else
        {
            SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
        }

        printf("Set coordinate fail, try again!\n");
    }
}

void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
    LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);

    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
        printf("Set imu rate success!\n");

        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate), LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
        printf("Set imu rate fail, try again!\n");
    }
}

/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
    LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }

    LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        if (response != 0)
        {
            p_lidar->connect_state = kConnectStateOn;
            printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", status, handle, response);
        }
        else
        {
            printf("Lidar start sample success\n");
        }
    }
    else if (status == kStatusTimeout)
    {
        p_lidar->connect_state = kConnectStateOn;
        printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n", status, handle, response);
    }
}

/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void* client_data)
{
}

/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char* bd_code)
{
    if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) || (whitelist_count_ >= kMaxLidarCount))
    {
        return -1;
    }

    if (LdsLidar::FindInWhitelist(bd_code))
    {
        printf("%s is alrealy exist!\n", bd_code);
        return -1;
    }

    strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
    ++whitelist_count_;

    return 0;
}

void LdsLidar::AddLocalBroadcastCode(void)
{
    for (size_t i = 0; i < sizeof(local_broadcast_code_list) / sizeof(intptr_t); ++i)
    {
        std::string invalid_bd = "000000000";
        printf("Local broadcast code : %s\n", local_broadcast_code_list[i]);
        if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) + 1) && (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str())))
        {
            LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
        }
        else
        {
            printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
        }
    }
}

bool LdsLidar::FindInWhitelist(const char* bd_code)
{
    if (!bd_code)
    {
        return false;
    }

    for (uint32_t i = 0; i < whitelist_count_; i++)
    {
        if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0)
        {
            return true;
        }
    }

    return false;
}

void LdsLidar::getcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out)
{
    cloud_mu.lock();
    pcl::copyPointCloud(*cloud, *cloud_out);
    cloud_mu.unlock();
}

void LdsLidar::getdata(PcloudI& cloud_out, Gydata& InputGydata)
{
    while (Icloud->points.size() != max_cloud_size)
    {
    }
    // cloud_mu.lock();

    cloud_mu.lock();
    // RoT_mu.lock();
    pcl::copyPointCloud(*Icloud, *cloud_out);
    InputGydata = OriginRoT;
    // outputRoT = OriginRoT;
    cloud_mu.unlock();
    // cloud_mu.unlock();
}

void LdsLidar::clearcloud()
{
    cloud_mu.lock();
    cloud->points.resize(max_cloud_size, pcl::PointXYZ(0, 0, 0));
    cloud_mu.unlock();
}

void LdsLidar::getall(std::vector<CloudI>& outputClouds, std::vector<Gydata>& outputGys)
{
    while (Iclouds.size() != CloudsNum_)
    {
        usleep(500000);
    }

    cloud_mu.lock();
    for (size_t i = 0; i < CloudsNum_; i++)
    {
        outputClouds[i] = Iclouds[i];
        outputGys[i] = Gydatas[i];
    }
    cloud_mu.unlock();
}

void LdsLidar::getCloudImg(PcloudI& cloud_out, cv::Mat& InputImg)
{
    cloud_mu.lock();
    pcl::copyPointCloud(*Icloud, *cloud_out);
    Eigen::Vector4f CamCoor;
    Eigen::Vector3f PxCoor;
    for (size_t i = 0; i < Icloud->size(); i++)
    {
        PointI tPoint = Icloud->points[i];
        float u, v;
        if (tPoint.x == 0 || tPoint.x > 10)
            continue;
        // if(abs(tPoint.y) > 6 || tPoint.z < -0.5 || tPoint.z > 1.7)
        //     continue;

        // LdCoor << tPoint.x, tPoint.y, tPoint.z, 1;
        CamCoor = RoT * Eigen::Vector4f(tPoint.x, tPoint.y, tPoint.z, 1);
        PxCoor = CamIntrix * Eigen::Vector3f(CamCoor.x(), CamCoor.y(), CamCoor.z());
        u = PxCoor(0) / CamCoor(2);
        v = PxCoor(1) / CamCoor(2);
        if (u < 0 || u > 1280 || v < 0 || v > 1024)
            continue;
        InputImg.at<int16_t>(v, u) = CamCoor(2) * 1000;
    }
    cloud_mu.unlock();
}